#include "inc/hw_memmap.h"
#include "gpio.h"
#include "Fault.h"
#include "Relay.h"
#include "LED.h"
#include "SI.h"
#include "I2C_IO_Driver.h"
#include "SOSZ.h"
#include "Button.h"
#include "h1010.h"
#include "main.h"

#include <string.h>

/*
 * MACRO DEFINITIONS AND ENUMERATIONS
 */
#define POWER_FAULT_PORT	GPIO_PORT_P4
#define LOCAL_GND_FLT_PIN	GPIO_PIN0
#define CHK_GF1_PIN			GPIO_PIN1
#define CHK_GF2_PIN			GPIO_PIN2
#define CHK_GF3_PIN			GPIO_PIN3
#define DC_FAULT_PIN		GPIO_PIN4
#define AC_FAULT_PIN		GPIO_PIN5
#define AC_F_PIN			GPIO_PIN7//GPIO_PIN6
#define DC_F_PIN			GPIO_PIN6//GPIO_PIN7
#define POWER_FAULT_PIN_MASK	(GPIO_PIN1 | GPIO_PIN2 | GPIO_PIN3 | GPIO_PIN4 | GPIO_PIN5 | GPIO_PIN6 | GPIO_PIN7)

enum powerFaultEnum {
	AC,
	DC,
	GND,
	Total
};
enum ePowerFault_state {
	ePowerFault_state_idle,
	ePowerFault_state_unacked,
	ePowerFault_state_acked,
	ePowerFault_state_clear
};

/*
 * STRUCTURES
 */
struct faultState_t;
typedef void (*pFxn_faultCheck)(struct faultState_t *);
struct faultState_t {
	enum SI_ADC_STATE faultState;
	BYTE isAcked;
	BYTE needLastAck;
	enum ePowerFault_state state;
	enum LED_INDEX myLed;
	BYTE faultBitMask;
	BYTE normalIsHigh;
	WORD timer;
	pFxn_faultCheck faultCheck;
};

struct powerFaultData_t {
	unsigned char inputMask;
	unsigned char driveMask;
	unsigned char pullUpMask;
	unsigned char pullDownMask;
	struct faultState_t state[Total];
};
struct SOfault_GlobalInfo_t {
	BYTE trigger;
	BYTE driveState;
	BYTE SOstatus;
	BYTE normalState;
};

/*
 * DATA
 */
extern const struct configMemRAM_t * pConfigMemRAM;
unsigned int timer = 0, lastTimer = 0;
enum e_mode modeSys = MODE_NORMAL;
struct faultState_t soFaultState[4];
struct powerFaultData_t powerFaultData;
struct SOfault_GlobalInfo_t soFaultGlobalInfo = {0,0};

/*
 * FUNCTION PROTOTYPTES
 */
void Fault_init(void);
void Fault_setMode(enum e_mode _mode);
void Fault_updateTimer(void);
void Fault_testThread(void);
void Fault_thread(void);

void Fault_stateInit(struct faultState_t * state, enum LED_INDEX myLed, BYTE  faultBitMask, BYTE normalIsHigh, pFxn_faultCheck faultCheck);
void Fault_state_SM(struct faultState_t * stateInfo);

void Fault_powerFault_init(void);
void Fault_powerFault_check(struct faultState_t * stateInfo);
void Fault_powerFault_test(void);

struct SOfault_GlobalInfo_t * SOfault_GlobalInfo_getSingleton(void);
BYTE SOfault_GlobalInfo_getTrigger(struct SOfault_GlobalInfo_t * p);
BYTE SOfault_GlobalInfo_getDriveState(struct SOfault_GlobalInfo_t * p);
BYTE SOfault_GlobalInfo_getSOstatus(struct SOfault_GlobalInfo_t * p);
void SOfault_GlobalInfo_updateTrigger(struct SOfault_GlobalInfo_t * p);
void SOfault_GlobalInfo_updateDriveState(struct SOfault_GlobalInfo_t * p);
void SOfault_GlobalInfo_updateSOstatus(struct SOfault_GlobalInfo_t * p);
void SOfault_GlobalInfo_update(struct SOfault_GlobalInfo_t * p);

void Fault_SOfault_init(void);
void Fault_SOfault_reset(void);
void Fault_SOfault_check(struct faultState_t * s);
void Fault_SOfault_test(void);

void Fault_init(void)
{
	Fault_powerFault_init();
	Fault_SOfault_init();
}
void Fault_stateInit(struct faultState_t * state, enum LED_INDEX myLed, BYTE  faultBitMask, BYTE normalIsHigh, pFxn_faultCheck faultCheck)
{
	state->faultState = SI_ADC_NORMAL;
	state->isAcked = 0;
	state->needLastAck = 0;
	state->timer = 0;
	state->state = ePowerFault_state_idle;
	state->myLed = myLed;
	state->faultBitMask = faultBitMask;
	state->normalIsHigh = normalIsHigh;
	state->faultCheck = faultCheck;
}
void Fault_powerFault_check(struct faultState_t * stateInfo);
//unsigned char inputMask = 0, driveMask = 0, pullUpMask = 0, pullDownMask = 0;
void Fault_powerFault_init(void)
{
	memset((BYTE*)&powerFaultData, 0, sizeof(powerFaultData));
//	inputMask = 0; driveMask = 0; pullUpMask = 0; pullDownMask = 0;
	if (HWTestEnabled == pConfigMemRAM->hwTests.InhibitACTest)
	{
		powerFaultData.driveMask |= AC_F_PIN;
		powerFaultData.inputMask |= AC_FAULT_PIN;
		powerFaultData.pullUpMask |= AC_FAULT_PIN;
//		powerFaultData.pullDownMask |= 0;
	}
	if (HWTestEnabled == pConfigMemRAM->hwTests.InhibitBatteryTest)
	{
		powerFaultData.driveMask |= DC_F_PIN;
		powerFaultData.inputMask |=  DC_FAULT_PIN;
		powerFaultData.pullUpMask |= DC_FAULT_PIN;
//		powerFaultData.pullDownMask |= 0;

	}
	if (HWTestEnabled == pConfigMemRAM->hwTests.InhibitGroundFaultTest)
	{
		powerFaultData.driveMask |= CHK_GF1_PIN | CHK_GF2_PIN | CHK_GF3_PIN;
		powerFaultData.inputMask |= LOCAL_GND_FLT_PIN;
//		powerFaultData.pullUpMask |= 0;
//		powerFaultData.pullDownMask |= 0;

	}
	if (!pConfigMemRAM->hwTests.InhibitUPSTest) {
		/// ?????
	}
	Fault_stateInit(&powerFaultData.state[AC], LED_PRIMARY, AC_FAULT_PIN, 1, Fault_powerFault_check);
	Fault_stateInit(&powerFaultData.state[DC], LED_STANDBY, DC_FAULT_PIN, 1, Fault_powerFault_check);
	Fault_stateInit(&powerFaultData.state[GND], LED_GROUND, LOCAL_GND_FLT_PIN, 0, Fault_powerFault_check);

	GPIO_setAsInputPin(POWER_FAULT_PORT, powerFaultData.inputMask);//BUTTON_MASK);
//	GPIO_setAsInputPinWithPullDownresistor(POWER_FAULT_PORT, powerFaultData.inputMask);//BUTTON_MASK);
	GPIO_setAsInputPinWithPullUpresistor(POWER_FAULT_PORT, powerFaultData.pullUpMask);
	GPIO_setAsOutputPin(POWER_FAULT_PORT, powerFaultData.driveMask);
	GPIO_setOutputHighOnPin(POWER_FAULT_PORT, powerFaultData.driveMask);
}
void Fault_powerFault_check(struct faultState_t * stateInfo)
{
	unsigned char result;
	result = GPIO_getInputPinValue(POWER_FAULT_PORT, stateInfo->faultBitMask);
//	result?(
	if (result)
	{
		stateInfo->normalIsHigh?(stateInfo->faultState = SI_ADC_FAULT):
				(stateInfo->faultState = SI_ADC_NORMAL);
	}
	else
	{
		stateInfo->normalIsHigh?(stateInfo->faultState = SI_ADC_NORMAL):
				(stateInfo->faultState = SI_ADC_FAULT);
	}
}
void Fault_state_SM(struct faultState_t * stateInfo)
{
	stateInfo->faultCheck(stateInfo);
	switch (stateInfo->state)
	{
		case ePowerFault_state_idle:
			LED_setRate(stateInfo->myLed, RATE_OFF);
			LED_resourceArray_requestRate(LED_GLOBAL_TROUBLE, stateInfo->myLed, RATE_OFF);
			REL_resourceArray_requestRelay(Relay_Fault, stateInfo->myLed, Relay_Off);
			if (stateInfo->faultState==SI_ADC_FAULT)
			{
				stateInfo->state = ePowerFault_state_unacked;
				LED_setRate(stateInfo->myLed, RATE_SLOW);
				LED_resourceArray_requestRate(LED_GLOBAL_TROUBLE, stateInfo->myLed, RATE_SLOW);
				REL_resourceArray_requestRelay(Relay_Fault, stateInfo->myLed, Relay_On);
			}
			break;
		case ePowerFault_state_unacked:
			LED_setRate(stateInfo->myLed, RATE_SLOW);
			LED_resourceArray_requestRate(LED_GLOBAL_TROUBLE, stateInfo->myLed, RATE_SLOW);
			REL_resourceArray_requestRelay(Relay_Fault, stateInfo->myLed, Relay_On);
			if (evt_yes == BTN_btnEvent_get(eBtnAck))
			{
				BTN_btnEvent_consume(eBtnAck);
				stateInfo->state = ePowerFault_state_acked;
				LED_setRate(stateInfo->myLed, RATE_ON);
				LED_resourceArray_requestRate(LED_GLOBAL_TROUBLE, stateInfo->myLed, RATE_ON);
				REL_resourceArray_requestRelay(Relay_Fault, stateInfo->myLed, Relay_On);
			}
			break;
		case ePowerFault_state_acked:
			LED_setRate(stateInfo->myLed, RATE_ON);
			LED_resourceArray_requestRate(LED_GLOBAL_TROUBLE, stateInfo->myLed, RATE_ON);
			REL_resourceArray_requestRelay(Relay_Fault, stateInfo->myLed, Relay_On);
			if (stateInfo->faultState==SI_ADC_NORMAL)
			{
				stateInfo->state = ePowerFault_state_clear;
				LED_setRate(stateInfo->myLed, RATE_SLOW_SLOW);
				LED_resourceArray_requestRate(LED_GLOBAL_TROUBLE, stateInfo->myLed, RATE_OFF);
				REL_resourceArray_requestRelay(Relay_Fault, stateInfo->myLed, Relay_Off);
			}
			break;
		case ePowerFault_state_clear:
			LED_setRate(stateInfo->myLed, RATE_SLOW_SLOW);
			LED_resourceArray_requestRate(LED_GLOBAL_TROUBLE, stateInfo->myLed, RATE_OFF);
			REL_resourceArray_requestRelay(Relay_Fault, stateInfo->myLed, Relay_Off);
			if (stateInfo->faultState==SI_ADC_FAULT)
			{
				stateInfo->state = ePowerFault_state_unacked;
				LED_setRate(stateInfo->myLed, RATE_SLOW);
				LED_resourceArray_requestRate(LED_GLOBAL_TROUBLE, stateInfo->myLed, RATE_SLOW);
				REL_resourceArray_requestRelay(Relay_Fault, stateInfo->myLed, Relay_On);
			}
			else if (evt_yes == BTN_btnEvent_get(eBtnAck))
			{
				BTN_btnEvent_consume(eBtnAck);
				stateInfo->state = ePowerFault_state_idle;
				LED_setRate(stateInfo->myLed, RATE_OFF);
				LED_resourceArray_requestRate(LED_GLOBAL_TROUBLE, stateInfo->myLed, RATE_OFF);
				REL_resourceArray_requestRelay(Relay_Fault, stateInfo->myLed, Relay_Off);
			}
			break;
	}
}
void Fault_powerFault_test(void)
{
	powerFaultData.state[AC].faultState = powerFaultData.state[DC].faultState = powerFaultData.state[GND].faultState = SI_ADC_NORMAL;
//	unsigned char result;// = GPIO_getInputPinValue(POWER_FAULT_PORT, powerFaultData.inputMask);
	REL_resourceArray_requestRelay(Relay_Fault, LED_PRIMARY, Relay_Off);
	REL_resourceArray_requestRelay(Relay_Fault, LED_STANDBY, Relay_Off);
	REL_resourceArray_requestRelay(Relay_Fault, LED_GROUND, Relay_Off);
	if (HWTestEnabled == pConfigMemRAM->hwTests.InhibitACTest)
	{
		Fault_state_SM(&powerFaultData.state[AC]);
	}
	if (HWTestEnabled == pConfigMemRAM->hwTests.InhibitBatteryTest)
	{
		Fault_state_SM(&powerFaultData.state[DC]);
	}
	if (HWTestEnabled == pConfigMemRAM->hwTests.InhibitGroundFaultTest)
	{
		Fault_state_SM(&powerFaultData.state[GND]);
	}
}
void Fault_setMode(enum e_mode _mode)
{
	modeSys = _mode;
}
void Fault_SOfault_init(void)
{
	Fault_SOfault_reset();
	Fault_stateInit(&soFaultState[0], LED_SO_1_Flt, SO_Flt_1, 1, Fault_SOfault_check);
	Fault_stateInit(&soFaultState[1], LED_SO_2_Flt, SO_Flt_2, 1, Fault_SOfault_check);
	Fault_stateInit(&soFaultState[2], LED_SO_3_Flt, SO_Flt_3, 1, Fault_SOfault_check);
	Fault_stateInit(&soFaultState[3], LED_SO_4_Flt, SO_Flt_4, 1, Fault_SOfault_check);
}
void Fault_SOfault_reset(void)
{
	LED_setRate(LED_SO_1_Flt, RATE_OFF);
	LED_setRate(LED_SO_2_Flt, RATE_OFF);
	LED_setRate(LED_SO_3_Flt, RATE_OFF);
	LED_setRate(LED_SO_4_Flt, RATE_OFF);
}
/*void Fault_btnAck(void)
{

}*/
struct SOfault_GlobalInfo_t * SOfault_GlobalInfo_getSingleton(void)
{
	return (&soFaultGlobalInfo);
}
BYTE SOfault_GlobalInfo_getTrigger(struct SOfault_GlobalInfo_t * p)
{
	return (p->trigger);
}
BYTE SOfault_GlobalInfo_getNormalState(struct SOfault_GlobalInfo_t * p)
{
	return (p->normalState);
}
BYTE SOfault_GlobalInfo_getDriveState(struct SOfault_GlobalInfo_t * p)
{
	return (p->driveState);
}
BYTE SOfault_GlobalInfo_getSOstatus(struct SOfault_GlobalInfo_t * p)
{
	return (p->SOstatus);
}
void SOfault_GlobalInfo_updateNormalState(struct SOfault_GlobalInfo_t * p)
{
	p->normalState = SOSZ_getSOSZ_SOnormalState();
}
void SOfault_GlobalInfo_updateTrigger(struct SOfault_GlobalInfo_t * p)
{
	p->trigger = SOSZ_getSOSZ_SOtrigger();
}
void SOfault_GlobalInfo_updateDriveState(struct SOfault_GlobalInfo_t * p)
{
	p->driveState = I2C_SO_getStatus();
}
void SOfault_GlobalInfo_updateSOstatus(struct SOfault_GlobalInfo_t * p)
{
	p->SOstatus = I2C_SO_getSOstatus();
}
void SOfault_GlobalInfo_update(struct SOfault_GlobalInfo_t * p)
{
	SOfault_GlobalInfo_updateNormalState(p);
	SOfault_GlobalInfo_updateTrigger(p);
	SOfault_GlobalInfo_updateDriveState(p);
	SOfault_GlobalInfo_updateSOstatus(p);
}
void Fault_SOfault_check(struct faultState_t * s)
{
	BYTE myTrigger = 0;
	BYTE myDrivenState = 0;
	BYTE myNormalState = 0;
	struct SOfault_GlobalInfo_t * p = SOfault_GlobalInfo_getSingleton();
	BYTE trigger = SOfault_GlobalInfo_getTrigger(p);//SOSZ_getSOSZ_SOtrigger();
	BYTE drivenState = SOfault_GlobalInfo_getDriveState(p);//I2C_SO_getStatus();
	BYTE soStat = SOfault_GlobalInfo_getSOstatus(p);
	BYTE soNormalState = SOfault_GlobalInfo_getNormalState(p);
	soStat &= s->faultBitMask;//mask;

	int i = s->myLed - LED_SO_1_Flt;

	myTrigger = trigger&(1<<i);
	myDrivenState = drivenState&(1<<i);
	myNormalState = soNormalState&(1<<i);

	if (myTrigger != myDrivenState)
	{
		/* TO DO: add timer to allow transition, then throw exception */
		__no_operation();
	}

	/* If in Service Mode, and SO is not configured to drive output in Service mode,
	 * must change the output state to OFF, since the output is not physically driven.
	 * The SO Fault circuitry is dependent on the state of the SO (on or off).
	 */
	if ((modeSys == MODE_SERVICE) &&
				(pConfigMemRAM->so[i].InhibitService == SO_configByte_InhibitService_isOff))
	{
		myTrigger = 0; // state is OFF, the SO will not be engaged, but the LED may be ON
	}
	// Test for SO Fault  here
	s->faultState = SI_ADC_NORMAL;
	if (myNormalState) // normally on
	{
		if (myTrigger == 0) // SO is logically off, but normal state is ON, so it is physically ON
		{
			if (s->timer < 100)
			{
				s->timer++;
			}
			else
			{
				if (!soStat)
				{
					s->faultState = SI_ADC_FAULT;
	//				s->timer++;
				}
				else
				{
					__no_operation();
				}
			}
		}
		else			// SO is logically on, but normal state is ON, so it is physically OFF
		{
			if ((soStat==s->faultBitMask)) {
				s->faultState = SI_ADC_FAULT;
			}
			s->timer = 0;
		}
	}
	else //if (myNormalState==0)  // normally off
	{
		if (myTrigger == 0) // SO is logically off, but normal state is OFF, so it is physically OFF
		{
			if ((soStat==s->faultBitMask)) {
				s->faultState = SI_ADC_FAULT;
			}
			s->timer = 0;
		}
		else			// SO is logically on, but normal state is OFF, so it is physically ON
		{
			if (s->timer < 100)
			{
				s->timer++;
			}
			else
			{
				if (!soStat)
				{
					s->faultState = SI_ADC_FAULT;
	//				s->timer++;
				}
				else
				{
					__no_operation();
				}
			}
		}
	}
}
void Fault_SOfault_test(void)//struct configMemRAM_t * pConfigMemRAM)//, unsigned char state)
{
	// function needs refactored, should use data sructure to pack fault mask and LED. I believe
	// this structure exists in SOSZ --> the LED_SO_2_Flt led reference is stored in the soPhyDriver object,
	// but this is not accessible from the public interface...
	struct SOfault_GlobalInfo_t * p = SOfault_GlobalInfo_getSingleton();
	SOfault_GlobalInfo_update(p);

	int i;
	for (i=0; i<4; i++)
	{
		/* If SO is disabled, don't test for SO Faults */
		if (pConfigMemRAM->so[i].OutputControl != SO_configByte_OutputControl_disabled)
		{
			// Test for SO Fault  here
//			Fault_SOfaultLogic(myState, SOfault[i].faultMask, SOfault[i].ledMap);*/
			Fault_state_SM(&soFaultState[i]);
		}
		else // do not test for SO Fault
		{
			LED_setRate(soFaultState[i].myLed, RATE_OFF);
			LED_resourceArray_requestRate(LED_GLOBAL_TROUBLE, soFaultState[i].myLed, RATE_OFF);
			REL_resourceArray_requestRelay(Relay_Fault, soFaultState[i].myLed, Relay_Off);
		}
	}
}
void Fault_updateTimer(void)
{
	timer++;
}
void Fault_testThread(void)
{
	Fault_SOfault_test();
	Fault_powerFault_test();
}
void Fault_thread(void)
{
	if (lastTimer == timer) return;
	if (modeSys == MODE_LAMP_TEST) return;

	Fault_SOfault_test();
	Fault_powerFault_test();
}
